#include <ros/ros.h>
#include "control/move_control/move_control.h"

using namespace control;

MoveControl::MoveControl(ros::NodeHandle &nh, ros::NodeHandle &private_nh)
: nh_(nh),
  private_nh_(private_nh)
{
    ptr_move_fsm_ = std::make_shared<FSM::MoveFsm>();
    ptr_move_fsm_->Init();//初始化底盘控制状态机
    //初始化到点阈值
    ptr_move_fsm_->InitFinishPoseThreshold(g_movefsm_tofinish_dis_precision, g_movefsm_tofinish_angle_precision);

    ptr_path_following_ = std::make_shared<LQRPathFollowing>(nh_, private_nh_);
    // ptr_path_following_ = std::make_shared<CFCPathFollowing>(nh_, private_nh_);

    ptr_trajectory_process_ = std::make_shared<Trajectory>();
    ptr_trajectory_process_->Init(nh_);
}

MoveControl::~MoveControl()
{
    
}

void MoveControl::Init(std::shared_ptr<AMRState> ptr_amr, std::shared_ptr<control_msgs::control_msg> ptr_control)
{
    ptr_amr_state_ = ptr_amr;
    ptr_ctrl_out_  = ptr_control;

    localization_record_.orientation = tf::createQuaternionMsgFromYaw(0.0f);
    srv_client_nearest_info_ = nh_.serviceClient<map_operation_msgs::GetNearestInfo>("/GetNearestInfoSrv");

    printf("SearchNearestPoint: %f %f\n", g_search_path_id_angle_thresh, g_search_path_id_dis_thresh);
}

void MoveControl::Process()
{
    // UpdateTrajectory();
    PreProcess();
    return;
}

void MoveControl::UpdateTrajectory()
{
    if (ptr_amr_state_ == nullptr)
        return;
    // 多线程需要加锁
    if (msg_task_.points.empty() 
        || msg_task_.points.size() != ptr_amr_state_->msg_trajectory_task.points.size()
        || msg_task_.points.back().pathid != ptr_amr_state_->msg_trajectory_task.points.back().pathid)
    {
        msg_task_ = ptr_amr_state_->msg_trajectory_task;
    }
}

void MoveControl::PreProcess()
{
    if (ptr_amr_state_ == nullptr)
        return;
    task_id_ = ptr_amr_state_->msg_trajectory_task.task_id;
    target_point_id_ = ptr_amr_state_->target_state.target_point_id;

    ptr_move_fsm_->EventHandle(ptr_amr_state_->msg_trajectory_task, *ptr_amr_state_); // TODO

    std::string cur_state = ptr_move_fsm_->GetCurState();
    if (cur_state == "Finish" || cur_state == "Fail")
    {
        ptr_move_fsm_->SetForceFinish(false);

        if (!ptr_amr_state_->msg_trajectory_loc.points.empty())
        {
            auto target_point = ptr_amr_state_->msg_trajectory_loc.points.rbegin();

            LINFO("TaskEnd: id {}, state {}", target_point->task_id, cur_state);
            LINFO("cur pose x:{}, y:{}, theta:{}", ptr_amr_state_->amr_local_.pose.position.x, ptr_amr_state_->amr_local_.pose.position.y, tf::getYaw(ptr_amr_state_->amr_local_.pose.orientation)); // TODO
            LINFO("target pose x:{} y:{}, theta:{}", target_point->pose.position.x, target_point->pose.position.y, target_point->amr_angle);
            ptr_amr_state_->target_state.target_point_pose = target_point->pose;
            if (cur_state == "Finish")
            {
                if (!ptr_amr_state_->msg_trajectory_task.points.empty())
                {
                    ptr_amr_state_->control_state.cur_point_id = ptr_amr_state_->msg_trajectory_task.points.back().waypoint_id;
                }
            }
            //结束的时候或失败，更新running_type // TODO
            ptr_path_following_->ReSet();

            last_min_index_ = -1;
            ptr_amr_state_->min_index = -1;
            ptr_amr_state_->msg_trajectory_task.points.clear();
            ptr_amr_state_->msg_trajectory_loc.points.clear();
            ptr_amr_state_->target_state.is_receive_cmd = false;
            ptr_amr_state_->lat_error = 0.0f;
            ptr_amr_state_->heading_error = 0.0f;
        }
    }

    if (ptr_move_fsm_->GetCurState() != "Work")
    {
        ptr_path_following_->VehicleStop();
    }
    else
    {
        CalcTrajectoryPro();
        if (!ptr_amr_state_->msg_trajectory_loc.points.empty())
        {
            // path is not reasonable 处理
            //路径跟踪控制
            ptr_path_following_->VehicleController(ptr_amr_state_->msg_trajectory_loc, *ptr_amr_state_, 
                                                ptr_path_following_->speed_, 
                                                ptr_path_following_->angle_speed_, 
                                                ptr_path_following_->speed_toward_,
                                                max_speed_limit_, 
                                                max_angle_speed_limit_);

            // ROS_INFO("move process3:  trajectory pro size: %d, speed: %f %f, angle speed:%f, speed_toward: %f", 
            //         (int)ptr_amr_state_->msg_trajectory_loc.points.size(), ptr_amr_state_->msg_trajectory_loc.points[0].target_speed,
            //         ptr_path_following_->speed_, ptr_path_following_->angle_speed_, ptr_path_following_->speed_toward_ );

            if (!ptr_amr_state_->amr_local_.valid_flag)
            {
                ptr_path_following_->VehicleStop();
                LINFO("localization live time too long, no confidence");
            }
        }
    }
    PostProcess();
    MsgTransfer();
}

void MoveControl::CalcTrajectoryPro()
{
    if (ptr_amr_state_ == nullptr)
    {
        return;
    }
    ptr_amr_state_->msg_trajectory_loc.points.clear();

    mtx_trajectory.lock();
    ptr_amr_state_->msg_trajectory_loc = ptr_trajectory_process_->TrajectoryProcess(ptr_amr_state_->msg_trajectory_task, *ptr_amr_state_, g_pick_up_length, last_min_index_);
    mtx_trajectory.unlock();
    
    ptr_amr_state_->min_index = last_min_index_;
    // ROS_INFO("CalcTrajectoryPro() --> %ld size: %d %d", last_min_index_, (int)ptr_amr_state_->msg_trajectory_task.points.size(), (int)ptr_amr_state_->msg_trajectory_loc.points.size());

    // static bool flag = true;
    // if (flag)
    // {
    //     for (auto point : ptr_amr_state_->msg_trajectory_loc.points)
    //     {
    //         ROS_INFO("point: x: %f, y: %f, loc: x: %f, y: %f, v: %f", 
    //                   point.pose.position.x, point.pose.position.y, point.loc_pose.position.x, point.loc_pose.position.y, point.target_speed);
    //     }
    //     flag = false;
    // }
}

void MoveControl::PostProcess()
{
    if (ptr_amr_state_ == nullptr)
        return;
    
    bool id_need_init = (ptr_amr_state_->control_state.cur_path_id == "-1" || ptr_amr_state_->control_state.cur_path_id == "");

    if (ptr_amr_state_->control_state.amr_fsm_state == "idle")
    {
        float dis = std::hypotf(localization_record_.position.x-ptr_amr_state_->amr_local_.pose.position.x, localization_record_.position.y-ptr_amr_state_->amr_local_.pose.position.y);
        // printf("dis: %f\n", dis);
        if (!ptr_amr_state_->amr_local_.valid_flag || dis > g_pathid_valid_dis)
        {
            init_path_id_ = true;
        }
    }

    bool search_period_meet = false;
    if ((ros::Time::now() - search_timestamp_).toSec() > 5.0f)
    {
        search_period_meet = true;
    }
    
    if ( (id_need_init || init_path_id_ || !ptr_amr_state_->actuator.is_auto_mode) && search_period_meet)
    {
        if (ptr_amr_state_->amr_local_.valid_flag)
        {
            // Call Service
            search_timestamp_ = ros::Time::now(); 
            map_operation_msgs::GetNearestInfo srv;
            if (GetPointInfoSrv(srv))
            {
                ptr_amr_state_->control_state.cur_path_id = srv.response.pathid;
                ptr_amr_state_->control_state.cur_point_id = srv.response.pointid;
                init_path_id_ = false;
            }
            else
            {
                ptr_amr_state_->control_state.cur_path_id = "-1";
                ptr_amr_state_->control_state.cur_point_id = "-1";
            }
        }
        else
        {
            ptr_amr_state_->control_state.cur_path_id = "-1";
            ptr_amr_state_->control_state.cur_point_id = "-1";
        }
    }
    else if (!ptr_amr_state_->msg_trajectory_loc.points.empty() && ptr_amr_state_->actuator.is_auto_mode)
    {
        ptr_amr_state_->control_state.cur_path_id = ptr_amr_state_->msg_trajectory_loc.points[0].pathid;
        ptr_amr_state_->control_state.cur_point_id = ptr_amr_state_->msg_trajectory_loc.points[0].waypoint_id;
        int first_point_task_id = ptr_amr_state_->msg_trajectory_loc.points[0].task_id;
        task_id_ = first_point_task_id > last_task_id_ ? first_point_task_id : last_task_id_;
        last_task_id_ = task_id_;
    }
    else
    {
    }

    localization_record_ = ptr_amr_state_->amr_local_.pose;
    // printf("PostProcess(), %s %s, id_need_init: %d, search_period_meet: %d, init_path_id_: %d, auto: %d, task_state: %s\n", 
    //         ptr_amr_state_->control_state.cur_path_id.c_str(), ptr_amr_state_->control_state.cur_point_id.c_str(), id_need_init, search_period_meet,
    //         init_path_id_, ptr_amr_state_->actuator.is_auto_mode, ptr_amr_state_->control_state.amr_fsm_state.c_str());
    LINFO("MoveControl::PostProcess() --> pathid: {}, pointid: {}", 
           ptr_amr_state_->control_state.cur_path_id.c_str(), ptr_amr_state_->control_state.cur_point_id.c_str());
}

bool MoveControl::GetPointInfoSrv(map_operation_msgs::GetNearestInfo &srv)
{
    if (!srv_client_nearest_info_.exists())
        return false;

    if (srv_client_nearest_info_.waitForExistence(ros::Duration(0.5f)))
    {
        int cnt = 0;
        srv.request.local_msg = ptr_amr_state_->amr_local_;
        srv.request.theta_thres = g_search_path_id_angle_thresh;
        srv.request.dis_thres   = g_search_path_id_dis_thresh;
        while (!srv_client_nearest_info_.call(srv) && (cnt < 5))
        {
            cnt++;
            if (cnt == 5)
            {
                printf("MoveControl::GetPointInfoSrv error, x: %f, y: %f, thres: %f %f\n", 
                       ptr_amr_state_->amr_local_.pose.position.x, ptr_amr_state_->amr_local_.pose.position.y,
                       g_search_path_id_angle_thresh, g_search_path_id_dis_thresh);
                // MoveControl::GetPointInfoSrv error, x: -0.061790, y: -0.192414, thres: 0.523599 0.500000
            }
            sleep(1);
        }
    }
    return srv.response.is_valid;
}

void MoveControl::GetTaskInfo(uint32_t &task_id, FsmState &task_state, ros::Time &task_time)
{
    task_id = task_id_;

    task_time = ptr_move_fsm_->GetTaskTime();
    
    std::string cur_fsm = ptr_move_fsm_->GetCurState();

    if (cur_fsm == "Work")
    {
        task_state = FsmState::Work;
    }
    else if (cur_fsm == "Finish")
    {
        task_state = FsmState::Finish;
    }
    else if (cur_fsm == "Pause")
    {
        task_state = FsmState::Pause;
    }
    else if (cur_fsm == "Fail")
    {
        task_state = FsmState::Fail;
    }
    else
    {
        task_state = FsmState::Other;
    }
}

void MoveControl::MsgTransfer()
{  
    ptr_ctrl_out_->speed = ptr_path_following_->speed_;
    ptr_ctrl_out_->angle_speed = ptr_path_following_->angle_speed_;
    ptr_ctrl_out_->speed_toward = ptr_path_following_->speed_toward_;

    ptr_ctrl_out_->lat_error = ptr_amr_state_->lat_error;
    ptr_ctrl_out_->heading_error = ptr_amr_state_->heading_error;
}